    .arm
    .syntax unified
    .cpu cortex-a7
    .equ  OS_SPSR_SYS_MODE, 0x1F
    .align 2
    .section .text
    .global init_timer_cp15 
    .global cp15_tick_handler
    .code 32
init_timer_cp15:
    stmfd sp!, {r0-r12,lr}
    bic r0, r0, #0x1f   
    orr r0, r0, #0x13
    msr cpsr, r0
    MRC p15, 0, r0, c14, c0, 0
    ldr r0, =62500000               /* 62.5KHz */
    MCR p15, 0, r0, c14, c0, 0 
    ldr r0, =6250000
    MCR p15, 0, r0, c14, c2, 0    /* CNTP_TVAL */
    MOV r0, #0x01            
    MCR p15, 0, r0, c14, c2, 1    /* CNTKCTL */
    CPSIE i
    ldmfd sp!,{r0-r12,pc} 

cp15_tick_handler:
    stmfd sp!, {r0-r12,lr}
    ldr    r0,  =g_tick_cnt 
    ldr    r1, [r0]
    add    r1, r1, #1
    str    r1, [r0]
    MOV r0, #0x00 
    MCR p15, 0, r0, c14, c2, 1    /* CNTKCTL */
    ldr r0, =6250000
    MCR p15, 0, r0, c14, c2, 0    /* CNTP_TVAL */

    MOV r0, #0x01 
    MCR p15, 0, r0, c14, c2, 1    /* CNTKCTL */
    ldmfd sp!,{r0-r12,pc} 




